﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using TrackingSim.Filtering.Kinematics;
using TrackingSim.Filtering.Measurements;
using TrackingSim.Filtering.Updating;
using TrackingSim.GameSystem;
using MathLib;

namespace TrackingSim.Filtering.Tracks
{
    class Track_R6 : Track
    {
        public Track_R6(double _time):base(_time)
        {
            KS = new State_R6(_time);
        }

        public override Track clone()
        {
            Track_R6 copy = new Track_R6(Time);

            copy.KS = KS.clone();

            return copy;
        }

        public override void init(Player p, Measurement meas)
        {
            State_R6 state = KS as State_R6;

            MathLib.Vector delta_body = meas.get_delta_body();
            MathLib.Vector delta_abs = Coords.abs_to_body(p).tp() * delta_body;

            state.State.X = p.Pos.X + delta_abs.X;
            state.State.Y = p.Pos.Y + delta_abs.Y;
            state.State.Z = p.Pos.Z + delta_abs.Z;

            state.Cov.m[0, 0] = 25.0;
            state.Cov.m[1, 1] = 25.0;
            state.Cov.m[2, 2] = 25.0;
            state.Cov.m[3, 3] = 9.0;
            state.Cov.m[4, 4] = 9.0;
            state.Cov.m[5, 5] = 9.0;
        }

        public override void visit(Razel razel, Player p)
        {
            State_R6 state = (State_R6)KS;

            double dt = razel.Time - Time;
            Time = razel.Time;

            MathLib.Matrix Q = new MathLib.Matrix(6, 6);
            Q.m[0, 0] = 100.0 * dt;
            Q.m[1, 1] = 100.0 * dt;
            Q.m[2, 2] = 100.0 * dt;

            extrap_covariance(this, dt, Q);
            extrap_state(this, dt);

            MathLib.Matrix z = razel.get_z();
            MathLib.Matrix z_est = R6_with_Razel.get_Z(this, p);
            MathLib.Matrix innovation = z - z_est;
            MathLib.Matrix R = razel.get_r();
            MathLib.Matrix H = R6_with_Razel.get_H(this, p);

            MathLib.Vector state_vector = new MathLib.Vector(6);
            MathLib.Matrix cov = new MathLib.Matrix(6, 6);
            state_vector.set_from(state.State);
            cov.set_from_matrix(state.Cov);

            state_vector = Coords.abs_to_body_6x6(p) * state_vector;
            cov = Coords.abs_to_body_6x6(p) * cov * Coords.abs_to_body_6x6(p).tp();

            Kalman.update_track_r6(ref state_vector, ref cov, innovation, H, R);

            state_vector = Coords.abs_to_body_6x6(p).tp() * state_vector;
            cov = Coords.abs_to_body_6x6(p).tp() * cov * Coords.abs_to_body_6x6(p);

            state.State.set_from(state_vector);
            state.Cov.set_from_matrix(cov);
        }

        public override void update(Measurement meas, Player p)
        {
            throw new NotImplementedException();
        }

        public override void update(Razel meas, Player p)
        {
            State_R6 state = (State_R6)KS;

            double dt = meas.Time - Time;
            Time = meas.Time;

            MathLib.Matrix Q = new MathLib.Matrix(6, 6);
            Q.m[0, 0] = 100.0 * dt;
            Q.m[1, 1] = 100.0 * dt;
            Q.m[2, 2] = 100.0 * dt;

            extrap_covariance(this, dt, Q);
            extrap_state(this, dt);

            MathLib.Matrix z = meas.get_z();
            MathLib.Matrix z_est = R6_with_Razel.get_Z(this, p);
            MathLib.Matrix innovation = z - z_est;
            MathLib.Matrix R = meas.get_r();
            MathLib.Matrix H = R6_with_Razel.get_H(this,p);

            MathLib.Vector state_vector = new MathLib.Vector(6);
            MathLib.Matrix cov = new MathLib.Matrix(6, 6);
            state_vector.set_from(state.State);
            cov.set_from_matrix(state.Cov);

            state_vector = Coords.abs_to_body_6x6(p) * state_vector;
            cov = Coords.abs_to_body_6x6(p) * cov * Coords.abs_to_body_6x6(p).tp();

            Kalman.update_track_r6(ref state_vector, ref cov, innovation, H, R);

            state_vector = Coords.abs_to_body_6x6(p).tp() * state_vector;
            cov = Coords.abs_to_body_6x6(p).tp() * cov * Coords.abs_to_body_6x6(p);

            state.State.set_from(state_vector);
            state.Cov.set_from_matrix(cov);
        }

        public static void extrap_covariance(Track_R6 track, double dt, MathLib.Matrix Q)
        {
            MathLib.Matrix phi = Kalman.phi_pos_vel_6state(dt);

            track.KS.Cov = phi * track.KS.Cov * phi.tp() + Q;
        }

        public static void extrap_state(Track_R6 track, double dt)
        {
            MathLib.Matrix phi = Kalman.phi_pos_vel_6state(dt);

            State_R6 state = (State_R6)track.KS;
            state.State = phi * state.State;

            double accel = 0.0;

            state.Xdot = state.Xdot + accel * dt;
            state.Ydot = state.Ydot + accel * dt;
            state.Zdot = state.Zdot + accel * dt;
        }
    }
}
